
package com.grt192.spot.controller;

import com.grt192.controller.pid.GRTPIDController;
import com.grt192.core.EventController;
import com.grt192.spot.actuator.GRTDemoLED;
import com.grt192.spot.mechanism.WallHugger.PIDDistanceDemoMech;
import com.grt192.spot.mechanism.hauntedHouse.StatusLight;

/**
 * A controller which regulates the operation of a PIDController
 * @author ajc
 */
public class PIDDistanceDemoControl extends EventController {

    public static final int DISTANCE = 10;
    public static final int SPEED = 100;
    private final PIDDistanceDemoMech mech;
    private GRTPIDController pid;
    double p = 1;
    double i = 0;
    double d = 0;
    private final StatusLight control;

    public PIDDistanceDemoControl(PIDDistanceDemoMech mech, GRTPIDController pid, StatusLight control) {
        this.mech = mech;
        this.pid = pid;
        pid.setContinuous();
        pid.setInputRange(0, 50);
        pid.setOutputRange(-500, 500);
        pid.setSetpoint(DISTANCE);
        setPrinting(true);
        this.control = control;
        control.rawColor(GRTDemoLED.Color.ORANGE);
    }

    public void startListening() {
        log("running!");
        control.rawColor(GRTDemoLED.Color.GREEN);
        pid.enable();
    }

    public void stopListening() {
        log("stopping!");
        control.rawColor(GRTDemoLED.Color.RED);
        pid.disable();
    }
}
